import math
import heapq
from tools import *



# 定义一个Node类，用于存储节点信息
class Node:
    # 初始化函数，传入节点id和cost
    def __init__(self, id, cost):
        self.id = id
        self.cost = cost
        self.parent = None
        self.g = math.inf
        self.f = math.inf

    # 重载小于号，用于比较两个节点的f值
    def __lt__(self, other):
        return self.f < other.f

def euclidean_distance(p1, p2):
    return math.sqrt((coordinates[p1][0] - coordinates[p2][0]) ** 2 + (coordinates[p1][1] - coordinates[p2][1]) ** 2)


def a_star(start, goal, adjacency_matrix):
    # 计算图中的节点数
    num_nodes = len(adjacency_matrix)
    # 初始化节点
    nodes = [Node(i, math.inf) for i in range(num_nodes)]
    # 初始化起点节点的g和f
    nodes[start].g = 0
    nodes[start].f = euclidean_distance(start, goal)

    # 初始化open_set和closed_set
    open_set = []
    closed_set = set()
    # 将起点节点加入open_set
    heapq.heappush(open_set, nodes[start])

    # 当open_set不为空时，循环
    while open_set:
        # 从open_set中取出当前节点
        current_node = heapq.heappop(open_set)

        # print(current_node.id, current_node.g, current_node.f)

        # 如果当前节点是目标节点，则打印路径，并返回
        if current_node.id == goal:
            path = []
            while current_node:
                path.append(current_node.id)
                current_node = current_node.parent
            path.reverse()
            return path

        # 将当前节点加入closed_set
        closed_set.add(current_node.id)

        # 遍历当前节点的所有邻居
        for neighbor in range(num_nodes):
            # 如果邻居节点与当前节点有连接，且未被访问过

            if adjacency_matrix[current_node.id][neighbor] > 0 and neighbor not in closed_set:
                # 初始化邻居节点
                neighbor_node = nodes[neighbor]
                # 计算 tentative_g
                tentative_g = current_node.g + adjacency_matrix[current_node.id][neighbor]

                # 如果 tentative_g 小于当前节点邻居节点的 g
                if tentative_g < neighbor_node.g:
                    # 将当前节点邻居节点的前驱节点设为当前节点
                    neighbor_node.parent = current_node
                    # 将当前节点邻居节点的 g 设为 tentative_g
                    neighbor_node.g = tentative_g
                    # 将当前节点邻居节点的f设为 tentative_g + 当前节点到目标节点的距离
                    neighbor_node.f = tentative_g + euclidean_distance(neighbor, goal)
                    # 如果当前节点邻居节点不在 open_set 中，则将当前节点邻居节点加入 open_set
                    if neighbor_node not in open_set:
                        heapq.heappush(open_set, neighbor_node)

    # 如果没有找到路径，则返回 None
    return None


# 示例使用

# 邻接矩阵表示图的代价
# adjacency_matrix = [
#     [0, 4, 2, 0, 0, 0],
#     [4, 0, 1, 5, 0, 0],
#     [2, 1, 0, 8, 10, 0],
#     [0, 5, 8, 0, 2, 6],
#     [0, 0, 10, 2, 0, 3],
#     [0, 0, 0, 6, 3, 0]
# ]
adjacency_matrix = getMartix()

#每个点的坐标
# coordinates = [
#     [1,1],
#     [1,2],
#     [1,3],
#     [2,1],
#     [2,2],
#     [2,3],
#     ]
coordinates = points

# 起点节点
start_node = 0
# 目标节点
goal_node = 34

# 调用 A* 算法，计算从起点到目标节点的路径
path = a_star(start_node, goal_node, adjacency_matrix)

# 如果找到路径，则打印路径
if path:
    print("Path:", path)
# 否则，打印没有找到路径
else:
    print("No path found.")

def getPath():
    return path